﻿
using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using Microsoft.Xna.Framework.Graphics;
using Microsoft.Kinect;
using Microsoft.Xna.Framework;

namespace Brain.Kinect
{
    /// <summary>
    /// A delegate method explaining how to map a SkeletonPoint from one space to another.
    /// </summary>
    /// <param name="point">The SkeletonPoint to map.</param>
    /// <returns>The Vector2 representing the target location.</returns>
    public delegate Vector2 SkeletonPointMap(SkeletonPoint point, KinectSensor sensor);

    public class KinectSkeletonDrawer : IDrawable
    {
        /// <summary>
        /// This is the map method called when mapping from
        /// skeleton space to the target space.
        /// </summary>
        protected SkeletonPointMap mapMethod;

        public KinectManager Manager { get; protected set; }

        public SpriteBatch SpriteBatch { get; set; }

        public bool ShouldDraw { get; set; }

        public Texture2D Empty;

        public Texture2D JointTexture;
        public Vector2 JointOrigin;

        public float BoneWidth = 4;

        public RenderTarget2D SkeletonTarget;

        public Vector2 Position;
        public Vector2 Scale = new Vector2(1);

        public KinectSkeletonDrawer(KinectManager manager,
            SpriteBatch sBatch, Texture2D JointTexture, Vector2 JointOrigin, SkeletonPointMap mapMethod)
        {
            this.JointOrigin = JointOrigin;
            this.Manager = manager;
            this.SpriteBatch = sBatch;
            this.mapMethod = mapMethod;

            ShouldDraw = true;

            this.JointTexture = JointTexture;

            Empty = new Texture2D(Engine.Instance.GraphicsDevice, 1, 1);
            Empty.SetData(new Color[] { Color.White });

            SkeletonTarget = new RenderTarget2D(Engine.Instance.GraphicsDevice, 640, 480, false, SurfaceFormat.Single, DepthFormat.None);
        }

        public void Draw()
        {
            if (Manager.ReadSkeleton)
            {
                Skeleton[] skeletons = Manager.GetSkeletonData();
                if (skeletons == null)
                {
                    return;
                }
                if (skeletons.Length > 0)
                {
                    Engine.Instance.GraphicsDevice.SetRenderTarget(SkeletonTarget);
                    Engine.Instance.GraphicsDevice.Clear(Color.Transparent);

                    SpriteBatch.Begin();
                }

                for (int i = 0; i < skeletons.Length; i++)
                {
                    Skeleton skeleton = skeletons[i];

                    switch (skeleton.TrackingState)
                    {
                        case SkeletonTrackingState.Tracked:
                            {
                                this.DrawBone(skeleton.Joints, JointType.Head, JointType.ShoulderCenter);
                                this.DrawBone(skeleton.Joints, JointType.ShoulderCenter, JointType.ShoulderLeft);
                                this.DrawBone(skeleton.Joints, JointType.ShoulderCenter, JointType.ShoulderRight);
                                this.DrawBone(skeleton.Joints, JointType.ShoulderCenter, JointType.Spine);
                                this.DrawBone(skeleton.Joints, JointType.Spine, JointType.HipCenter);
                                this.DrawBone(skeleton.Joints, JointType.HipCenter, JointType.HipLeft);
                                this.DrawBone(skeleton.Joints, JointType.HipCenter, JointType.HipRight);

                                this.DrawBone(skeleton.Joints, JointType.ShoulderLeft, JointType.ElbowLeft);
                                this.DrawBone(skeleton.Joints, JointType.ElbowLeft, JointType.WristLeft);
                                this.DrawBone(skeleton.Joints, JointType.WristLeft, JointType.HandLeft);

                                this.DrawBone(skeleton.Joints, JointType.ShoulderRight, JointType.ElbowRight);
                                this.DrawBone(skeleton.Joints, JointType.ElbowRight, JointType.WristRight);
                                this.DrawBone(skeleton.Joints, JointType.WristRight, JointType.HandRight);

                                this.DrawBone(skeleton.Joints, JointType.HipLeft, JointType.KneeLeft);
                                this.DrawBone(skeleton.Joints, JointType.KneeLeft, JointType.AnkleLeft);
                                this.DrawBone(skeleton.Joints, JointType.AnkleLeft, JointType.FootLeft);

                                this.DrawBone(skeleton.Joints, JointType.HipRight, JointType.KneeRight);
                                this.DrawBone(skeleton.Joints, JointType.KneeRight, JointType.AnkleRight);
                                this.DrawBone(skeleton.Joints, JointType.AnkleRight, JointType.FootRight);

                                // Now draw the joints
                                foreach (Joint j in skeleton.Joints)
                                {
                                    Color jointColor = Color.Green;
                                    if (j.TrackingState != JointTrackingState.Tracked)
                                    {
                                        jointColor = Color.Yellow;
                                    }

                                    this.SpriteBatch.Draw(
                                        this.JointTexture,
                                        this.mapMethod(j.Position, this.Manager.Sensor),
                                        null,
                                        jointColor,
                                        0.0f,
                                        JointOrigin,
                                        1.0f,
                                        SpriteEffects.None,
                                        0.0f);
                                }
                            }
                            break;
                        case SkeletonTrackingState.PositionOnly:
                            {
                                // If we are only tracking position, draw a blue dot
                                this.SpriteBatch.Draw(
                                        this.JointTexture,
                                        this.mapMethod(skeleton.Position, this.Manager.Sensor),
                                        null,
                                        Color.Blue,
                                        0.0f,
                                        Vector2.Zero,
                                        1.0f,
                                        SpriteEffects.None,
                                        0.0f);
                            }
                            break;
                    }
                }

                if (skeletons.Length > 0)
                {
                    SpriteBatch.End();

                    Engine.Instance.GraphicsDevice.SetRenderTarget(null);
                }
            }
        }

        /// <summary>
        /// This method draws a bone.
        /// </summary>
        /// <param name="joints">The joint data.</param>
        /// <param name="startJoint">The starting joint.</param>
        /// <param name="endJoint">The ending joint.</param>
        private void DrawBone(JointCollection joints, JointType startJoint, JointType endJoint)
        {
            Vector2 start = this.mapMethod(joints[startJoint].Position, this.Manager.Sensor);
            Vector2 end = this.mapMethod(joints[endJoint].Position, this.Manager.Sensor);
            Vector2 diff = end - start;
            Vector2 scale = new Vector2(BoneWidth, diff.Length() / this.Empty.Height);

            float angle = (float)Math.Atan2(diff.Y, diff.X) - MathHelper.PiOver2;

            Color color = Color.LightGreen;
            if (joints[startJoint].TrackingState != JointTrackingState.Tracked ||
                joints[endJoint].TrackingState != JointTrackingState.Tracked)
            {
                color = Color.Gray;
            }

            this.SpriteBatch.Draw(Empty, start, null, color, angle, Vector2.Zero, scale, SpriteEffects.None, 1.0f);
        }
    }
}